/*
 * Chris Bena
 * 4/1/11
 * 
 * Converted old wall follow code to the new
 *  version of player with hokuyo_aist instead
 *  of laser proxy.
 *
 * KNOWN ISSUES:
 *  file output - opening the file but not writing
 *  occasional script version error - run again
 *  
 */
#include <libplayerc++/playerc++.h>
#include <iostream>
#include <fstream>
#include <stdio.h>
#include <math.h>

#include "args.h"
#include <hokuyo_aist/hokuyo_aist.h>
#include <hokuyo_aist/hokuyo_errors.h>
#include <flexiport/flexiport.h>

#define FRONT 340
#define RIGHT 85
#define LEFT 595
#define FRONT_RIGHT 212
#define FRONT_LEFT 465
#define MIN_RANGE 500 // min range on the right
#define MAX_RANGE 1000 // max range on the right
#define COLLISION 350 // the min range for collision detection
#define LOWERLIMIT 80 // lower limit to ignore garbage laser readings

// max range set to 1.5m for stage; on robot it 
// should be about 0.8-1.0m

using namespace PlayerCc;
    
PlayerClient robot(gHostname, gPort);
Position2dProxy pp(&robot, gIndex);
//LaserProxy lp(&robot, gIndex);
hokuyo_aist::ScanData data;
hokuyo_aist::Sensor laser;
std::ofstream fout;
bool door = false;

      std::string port_options("type=serial,device=/dev/ttyACM0,timeout=1");
      double start_angle(0.0), end_angle(0.0);
      int first_step(-1), last_step(-1);
      int multiecho_mode(0);
      unsigned int baud(19200), speed(0), cluster_count(1);
      bool get_intensities(false), get_new(false), verbose(false);

void turn(double newspeed, double newturnrate){
  pp.SetSpeed(newspeed, newturnrate);
}

// Turns left - turns until lp[FRONT] is maxed out;
void turn(){
  do{
    //pp.SetSpeed(0.1, -0.75);
    pp.SetSpeed(0.0, 0.375);
    laser.get_ranges(data,-1,-1,cluster_count);
    std::cout << "Still turning... " << data[FRONT] << "\n"; 
  }
  while(data[FRONT] < 5580 && data[FRONT]>=LOWERLIMIT);
}

void saveData(){
  double x = pp.GetXPos();
  double y = pp.GetYPos();
  double yaw = pp.GetYaw();
  std::cout << "output " << "x: " << x 
	    <<" y: " <<y 
	    <<" yaw: " <<rtod(yaw)<< "\n";

  //fout << "X: "; 
  //fout.width(13); 
  fout << x; 
  fout << " ";

  //fout << "Y: ";
  //fout.width(13);
  fout << y;
  fout << " ";

  //fout << "Yaw: ";
  //fout.width(13);
  fout << rtod(yaw);
  fout << " ";

  //fout << "Right: "; 
  fout << data[RIGHT]/1000;
  fout << " ";

  //fout << "Left: ";
  fout << data[LEFT]/1000 << "\n";
}

int main(int argc, char **argv)
{
  parse_args(argc,argv);
  
  // we throw exceptions on creation if we fail
  try
    {
      pp.SetMotorEnable (true);
      
      double newspeed = 0;
      double newturnrate = 0;
      
      //robot.Read(); //here so GetRange doesn't segfault
      

      /* Setup laser 
       *
       *  The following is mostly error checking and 
       *   system information.
       */
      
   
#if defined(WIN32)
      port_options = "type=serial,device=COM3,timeout=1";
#else
      int opt;
      // Get some options from the command line
      while((opt = getopt(argc, argv, "b:c:e:f:il:m:no:s:u:vh")) != -1)
	{
	  switch(opt)
	    {
	    case 'b':
	      sscanf(optarg, "%d", &baud);
	      break;
	    case 'c':
	      sscanf(optarg, "%d", &cluster_count);
	      break;
	    case 'e':
	      sscanf(optarg, "%lf", &end_angle);
	      break;
	    case 'f':
	      sscanf(optarg, "%d", &first_step);
	      break;
	    case 'i':
	      get_intensities = true;
	      break;
	    case 'l':
	      sscanf(optarg, "%d", &last_step);
	      break;
	    case 'm':
	      sscanf(optarg, "%d", &speed);
	      break;
	    case 'n':
	      get_new = true;
	      break;
	    case 'o':
	      port_options = optarg;
	      break;
	    case 's':
	      sscanf(optarg, "%lf", &start_angle);
	      break;
	    case 'u':
	      sscanf(optarg, "%d", &multiecho_mode);
	      break;
	    case 'v':
	      verbose = true;
	      break;
	    case '?':
	    case 'h':
	    default:
	      std::cout << "Usage: " << argv[0] << " [options]\n\n";
	    std::cout << "-b baud\t\tBaud rate to set the laser to "
	      "*after* connecting.\n";
	    std::cout << "-c count\tCluster count.\n";
	    std::cout << "-e angle\tEnd angle to get ranges to.\n";
	    std::cout << "-f step\t\tFirst step to get ranges from.\n";
	    std::cout << "-i\t\tGet intensity data along with ranges.\n";
	    std::cout << "-l step\t\tLast step to get ranges to.\n";
	    std::cout << "-m speed\tMotor speed.\n";
	    std::cout <<
	      "-n\t\tGet new ranges instead of latest ranges.\n";
	    std::cout <<
	      "-o options\tPort options (see flexiport library).\n";
	    std::cout << "-s angle\tStart angle to get ranges from.\n";
	    std::cout << "-u mode\tMulti-echo detection:\n";
	    std::cout << "\t\t0: Off (default), 1: Front, 2: Middle, "
	      "3: Rear, 4: Average\n";
	    std::cout <<
	      "-v\t\tPut the hokuyo_aist library into verbose mode.\n";
	    return 1;
	    }
	}
#endif // defined(WIN32)
      
      //hokuyo_aist::Sensor laser;
      if (verbose)
	laser.set_verbose(true);  // more information in the console
      laser.open(port_options); // Open the later
      
      // Calibration
      std::cout << "Calibrating laser time\n";
      laser.calibrate_time();
      std::cout << "Calculated offset: " << laser.time_offset() << "ns\n";
      std::cout << "Calculated drift rate: " << laser.drift_rate() << '\n';
      std::cout << "Calculated skew alpha: " << laser.skew_alpha() << '\n';
      
      // Turn on the laser
      laser.set_power(true);
      try
	{
	  laser.set_baud(baud);
	}
      catch(hokuyo_aist::BaudrateError &e)
	{
	  std::cerr << "Failed to change baud rate: " << e.what() << '\n';
	}
      catch(hokuyo_aist::ResponseError &e)
	{
	  std::cerr << "Failed to change baud rate: " << e.what() << '\n';
	}
      catch(...)
	{
	  std::cerr << "Failed to change baud rate\n";
	}
      // Set the motor speed
      try
	{
	  laser.set_motor_speed(speed);
	}
      catch(hokuyo_aist::MotorSpeedError &e)
	{
	  std::cerr << "Failed to set motor speed: " << e.what() << '\n';
	}
      catch(hokuyo_aist::ResponseError &e)
	{
	  std::cerr << "Failed to set motor speed: " << e.what() << '\n';
	}
      switch(multiecho_mode)
	{
	case 1:
	  laser.set_multiecho_mode(hokuyo_aist::ME_FRONT);
	  break;
	case 2:
	  laser.set_multiecho_mode(hokuyo_aist::ME_MIDDLE);
	  break;
	case 3:
	  laser.set_multiecho_mode(hokuyo_aist::ME_REAR);
	  break;
	case 4:
	  laser.set_multiecho_mode(hokuyo_aist::ME_AVERAGE);
	  break;
	case 0:
	default:
	  laser.set_multiecho_mode(hokuyo_aist::ME_OFF);
	  break;
	}
      // Get some laser info
      std::cout << "Laser sensor information:\n";
      hokuyo_aist::SensorInfo info;
      laser.get_sensor_info(info);
      std::cout << info.as_string();

      /*  Laser Setup Complete */


      /* Open file out */

      fout.open("7_0_OutputData.txt");
      if(!fout.is_open()){
	std::cerr << "Error opening file\n";
	exit(-1);
      }

      fout.setf(std::ios::fixed, std::ios::floatfield);
      fout.precision(8);

      /* File open complete */
    
      pp.ResetOdometry();

      for(;;)
	{
	
	  // this blocks until new data comes; 10Hz by default
	  laser.get_ranges(data,-1,-1,cluster_count);
	  robot.Read(); 
	  // Using only the certain sensors - front, left, right
	  double front = data[FRONT];
	  double frontRight = data[FRONT_RIGHT];
	  double frontLeft = data[FRONT_LEFT];
	  double right = data[RIGHT];
	  double left = data[LEFT];
	
	
	  double yaw = pp.GetYaw();
	  double minRight = 0;
	  double minDistRight = 5600;
	  double minDistLeft = 5600;
	
	
	  for(int i = RIGHT; i <= FRONT; i++){
	    if(data[i] < minDistRight && data[i]>=LOWERLIMIT){
	      minDistRight=data[i];
	      minRight=i;
	    }
	  }
	  for(int i = FRONT; i <= LEFT; i++){
	    if(data[i] < minDistLeft && data[i]>=LOWERLIMIT)
	      minDistLeft=data[i];
	  }
	
	
	  // if inside range
	  if(frontRight <= MIN_RANGE || minDistRight <= COLLISION){
	    std::cout << "inside of range  " << frontRight << " " << minDistRight << "\n";
	    newspeed = 0.0;
	    newturnrate = 0.175;
	    turn(newspeed, newturnrate);
	  }
	
	  // else if outside range
	  if(frontRight > MAX_RANGE || minDistLeft <= COLLISION){
	    std::cout << "Outside range" << frontRight << " " << minDistLeft << "\n";
	    //std::cout << frontRight << "\n";
	    newspeed = 0.25;
	    newturnrate = -0.190;
	    turn(newspeed, newturnrate);
	  }
	
	  //}
	
	  // Will hit this if there is a door (opened or closed)
	  //  or if it's in the appropriate range
	  else{
	    std::cout << "Straight ahead\n";
	    newspeed = 0.5;
	    newturnrate = 0.0;
	    turn(newspeed, newturnrate);
	  }
	
	  // if reached a corner
	  if(front <= 1500 && frontRight <= 800 && front>=LOWERLIMIT && frontRight>=LOWERLIMIT){
	    std::cout << "Turning a corner " << front << " " << frontRight << "\n";
	    turn();
	  }
	
	  // when we test for front <= 1.0 it gets very close to the wall 
	  // and then starts to turn.  The reason is that there's a while
	  // between when the condition turns true and the robot responds
	  // to the new turn command.
	
	  saveData();
	  /**/
	}
    
    }
  
  catch (PlayerCc::PlayerError e)
    {
      std::cerr << e << std::endl;
      return -1;
    }
}
  
